﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace agvcontroller.Algorothm
{
    public enum enumAlgorithm
    {
        FLOYD,
        DIJKSTRA,
    }

    /// <summary>
    /// Dikstra Algorithm
    /// </summary>
    public class CPathFinder
    {
        public enumAlgorithm _Algorithm;
        int[,] P;
        int[,] D;
        int INFINIRY = 15000;
        List<int> shortestPath = new List<int>();

        public CPathFinder()
        {
            _Algorithm = enumAlgorithm.FLOYD;
        }

        public CPathFinder(enumAlgorithm algorithm)
        {
            _Algorithm = algorithm;
        }

        public void Reset()
        {
            shortestPath.Clear();
        }

        public int[] SearchShortestPath(int[,] metrix, int source, int dest, out int distance)
        {
            Reset();

            switch (_Algorithm)
            {
                case enumAlgorithm.FLOYD:
                    Floyd(metrix, source, dest);
                    break;
                case enumAlgorithm.DIJKSTRA:
                    Dijkstra(metrix, source, dest);
                    break;
            }

            distance = D[source, dest];
            return MakePath(source, dest);
        }

        public int[] SearchShortestPath(int[,] metrix, int source, int dest)
        {
            int distance;
            return SearchShortestPath(metrix, source, dest, out distance);
        }

        public int[] SearchShortestPath(int[,] metrix, int source, int middle, int dest, out int distance)
        {
            int[] path1, path2, path;
            int distance1, distance2;

            path1 = SearchShortestPath(metrix, source, middle, out distance1);
            path2 = SearchShortestPath(metrix, middle, dest, out distance2);

            if (distance1 != INFINIRY && distance2 != INFINIRY)
            {
                distance = distance1 + distance2;
                path = CombinePath(path1, path2);
            }
            else
            {
                distance = 0;
                path = (new List<int>()).ToArray();
            }

            return path;
        }

        public void Floyd(int[,] metrix, int source, int dest)
        {
            int n = metrix.GetLength(0);

            D = new int[n, n]; // 인접 행렬을 받기 위한 배열 생성
            P = new int[n, n]; // 최적 경로 행렬 생성

            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    P[i, j] = -1;

                    D[i, j] = metrix[i, j];

                    if (D[i, j] <= 0)
                        D[i, j] = 15000;
                }
            }

            for (int k = 0; k < n; k++)
            {
                for (int i = 0; i < n; i++)
                {
                    for (int j = 0; j < n; j++)
                    {
                        if (D[i, k] + D[k, j] < D[i, j])
                        {
                            P[i, j] = k; //최단 경로 저장
                            D[i, j] = D[i, k] + D[k, j]; //최단 경로 길이 계산
                        }
                    }
                }
            }
        }

        public void Dijkstra(int[,] metrix, int source, int dest)
        {

        }

        public int[] MakePath(int source, int dest)
        {
            TracePath(source, dest);

            if (shortestPath.Count != 0 || D[source, dest] < 15000)
            {
                shortestPath.Insert(0, source);
                shortestPath.Add(dest);
            }

            return shortestPath.ToArray();
        }

        public void TracePath(int source, int dest)
        {
            if (P[source, dest] >= 0)
            {
                TracePath(source, P[source, dest]);
                shortestPath.Add(P[source, dest]);
                TracePath(P[source, dest], dest);
            }
        }

        public int[] CombinePath(int[] path1, int[] path2)
        {
            int[] result = new int[path1.Length + path2.Length - 1];

            for (int i = 0; i < path1.Length; i++)
            {
                result[i] = path1[i];
            }

            for (int i = 1; i < path2.Length; i++)
            {
                result[(path1.Length - 1) + i] = path2[i];
            }

            return result;
        }
    }
}
